#include<ros/ros.h>  // 引入 ROS 库，提供 ROS 节点功能
#include<sensor_msgs>  // 引入 sensor_msgs 库，用于 PointCloud2 消息
#include<pcl_conversions/pcl_conversions.h>  // 引入 pcl_conversions 库，用于 PointCloud2 与 PointCloud 之间的转换
#include<pcl/point_cloud.h>  // 引入 pcl 库，用于处理点云
#include<pcl/conversions.h>  // 引入 pcl 库，用于 PointCloud2 与 PointCloud 之间的转换
#include<pcl/filters/passthrough.h>  // 引入 pcl 库，用于点云过滤器，允许根据条件通过点云数据

ros::Subscriber point_cloud_sub;
std::vector<std::vector<float>> global_obs;
std::vector<std::vector<float>> static_obs;

//读取离线静态障碍数据
bool loadobstacledata(const std::string& filename,std::vector<std::vector<float>> &static_obs)
{
    std::ifstream file(filename);
    if(!file.is_open())
    {
        ROS_ERROR("failed to open the static_obs file: %s",filename.c_str());
    }
    float x,y;
    while(file>>x>>y)
    {
        static_obs.push_back({x,y});
    }
    file.close();
    return true;
}
// 定义一个回调函数来处理接收到的 PointCloud2 消息
void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& msg)
{
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*msg, *cloud);
    global_obs.clear();
    // 遍历点云数据中的所有点,添加到全局变量global_obs中
    for (const auto& point : cloud->points)
    {
        std::vector<float> obstacle = {point.x, point.y};
        global_obs.push_back(obstacle); 
    }
    //将离线障碍数据添加到obs里面
    for(const auto& obs_temp : static_obs)
    {
        global_obs.push_back(obs_temp);
    }



    ROS_INFO("Extracted %zu obstacles.", obs.size()); 
    // 打印每个障碍物的坐标
    for (const auto point : global_obs)
    {
        ROS_INFO("Obstacle at (x: %f, y: %f)", point[0], point[1]);
    }
}

void publish_obstacles()
{
    std_msgs::Float32MultiArray msg;
    std::vector<float> obs_data;
    for(const auto& obst:global_obs)
    {
        obs_data.push_back(obst[0]);
        obs_data.push_back(obst[1]);
    
    }
    msg.data=obs_data;
    obs_pub.publish(msg);
    ROS_INFO("Published %zu obstacles.", global_obs.size());
}
int main(int argc, char** argv)
{
    //读取静态障碍数据加载到static_obs变量中。
    string file_name="/ptcmap";
    bool loadobstacledata(file_name,static_obs);
    // 初始化 ROS 节点，节点名称为 "dynamic_obstacle"
    ros::init(argc, argv, "dynamic_obstacle");
    ros::NodeHandle nh;
    obs_pub=nh.advertise<std_msgs::Float32MultiArray>("/planningobs0_topic",10);
    // 订阅 "/total_Material_topic" 话题，注册回调函数 pointCloudCallback
    ros::Subscriber sub = nh.subscribe("/total_Material_topic", 1, pointCloudCallback);
    // 设置 ROS 循环的频率为 0.25 Hz（每秒 0.25 次）
    ros::Rate rate(0.25); 

    // 在 ROS 节点运行期间，不断调用回调函数，直到节点关闭
    while (ros::ok())
    {
        publish_obstacles();  // 发布障碍物坐标
        ros::spinOnce();  // 调用一次回调函数
        rate.sleep();  // 按照设定的频率休眠
    }

    return 0;  // 返回 0，程序结束
}
